#include "Nano100Series.h"
#include "UsbTransfer.h"
#include "HmdCmd.h"
#include "EventQueue.h"

static uint32_t u32TxSize2 = 0;
static uint32_t u32TxSize3 = 0;
static uint32_t u32TxSize4 = 0;
static uint32_t u32TxSize5 = 0;
static uint32_t u32TxSize6 = 0;
//static uint32_t u32TxSize7 = 0;
static uint8_t  u8TxBuf2[64];
static uint8_t  u8TxBuf3[64];
static uint8_t  u8TxBuf4[64];
static uint8_t  u8TxBuf5[64];
static uint8_t  u8TxBuf6[64];
//static uint8_t  u8TxBuf7[64];

//static uint32_t u32RxSize2 = 0;
static uint32_t u32RxSize3 = 0;
//static uint32_t u32RxSize4 = 0;
static uint32_t u32RxSize5 = 0;
//static uint32_t u32RxSize6 = 0;
static uint32_t u32RxSize7 = 0;
//static uint8_t  u8RxBuf2[64];
static uint8_t  u8RxBuf3[64];
//static uint8_t  u8RxBuf4[64];
static uint8_t  u8RxBuf5[64];
//static uint8_t  u8RxBuf6[64];
static uint8_t  u8RxBuf7[64];

static int epCount[8] = {0};

int usb_interface_alt = 0;

void Timer1Open(void);
void Timer1Close(void);

/*--------------------------------------------------------------------------*/
void USBD_IRQHandler(void)
{
	int i;
    uint32_t u32IntSts = USBD_GET_INT_FLAG();
    uint32_t u32State = USBD_GET_BUS_STATE();

	for (i = 0; i < 8; i++) {
		if (u32IntSts & (1UL << (16 + i))) {
			epCount[i]++;
		}
	}
	//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_FLDET) {
        // Floating detect
        USBD_CLR_INT_FLAG(USBD_INTSTS_FLDET);

        if (USBD_IS_ATTACHED()) {
            /* USB Plug In */
            USBD_ENABLE_USB();
        } else {
            /* USB Un-plug */
            USBD_DISABLE_USB();
        }
    }

	//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_BUS) {
        /* Clear event flag */
        USBD_CLR_INT_FLAG(USBD_INTSTS_BUS);

        if (u32State & USBD_STATE_USBRST) {
            /* Bus reset */
            USBD_ENABLE_USB();
            USBD_SwReset();
			EventQueueInit(&hmd_event_queue, &(hmd_events[0]), HMD_EVENT_SIZE);
        }
        if (u32State & USBD_STATE_SUSPEND) {
            /* Enable USB but disable PHY */
            USBD_DISABLE_PHY();
        }
        if (u32State & USBD_STATE_RESUME) {
            /* Enable USB and enable PHY */
            USBD_ENABLE_USB();
        }
    }

	//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_USB) {
        // USB event
        if (u32IntSts & USBD_INTSTS_SETUP) {
            // Setup packet
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_SETUP);

            /* Clear the data IN/OUT ready flag of control end-points */
            USBD_STOP_TRANSACTION(EP0);
            USBD_STOP_TRANSACTION(EP1);

            USBD_ProcessSetupPacket();
        }

        // EP events
        if (u32IntSts & USBD_INTSTS_EP0) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP0);

            // control IN
            USBD_CtrlIn();
        }

        if (u32IntSts & USBD_INTSTS_EP1) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP1);

            // control OUT
            USBD_CtrlOut();
        }

        if (u32IntSts & USBD_INTSTS_EP2) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP2);
        }

        if (u32IntSts & USBD_INTSTS_EP3) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP3);
        }

        if (u32IntSts & USBD_INTSTS_EP4) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP4);
        }

        if (u32IntSts & USBD_INTSTS_EP5) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP5);
        }

        if (u32IntSts & USBD_INTSTS_EP6) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP6);
        }

        if (u32IntSts & USBD_INTSTS_EP7) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP7);
        }
    }
    /* clear unknown event */
	
    USBD_CLR_INT_FLAG(u32IntSts);
}

void UsbClassRequest(void)
{
    uint8_t buf[8];

    USBD_GetSetupPacket(buf);
}

static void UsbSetAltInterface0(void)
{
	/* EP2 ==> Bulk IN endpoint, address 1 */
    USBD_CONFIG_EP(EP2, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_1);
    /* Buffer offset for EP2 */
    USBD_SET_EP_BUF_ADDR(EP2, EP2_BUF_BASE);

    /* EP3 ==> Bulk Out endpoint, address 1 */
    USBD_CONFIG_EP(EP3, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_1);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP3, EP3_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP3, EP3_MAX_PKT_SIZE);

    /* EP4 ==> Bulk IN endpoint, address 2 */
    USBD_CONFIG_EP(EP4, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_2);
    /* Buffer offset for EP4 */
    USBD_SET_EP_BUF_ADDR(EP4, EP4_BUF_BASE);

    /* EP5 ==> Bulk Out endpoint, address 2 */
    USBD_CONFIG_EP(EP5, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_2);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP5, EP5_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP5, EP5_MAX_PKT_SIZE);

	/* EP6 ==> Bulk IN endpoint, address 3 */
    USBD_CONFIG_EP(EP6, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_3);
    /* Buffer offset for EP6 */
    USBD_SET_EP_BUF_ADDR(EP6, EP6_BUF_BASE);

    /* EP7==> Bulk Out endpoint, address 3 */
    USBD_CONFIG_EP(EP7, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_3);
    /* Buffer offset for EP7 */
    USBD_SET_EP_BUF_ADDR(EP7, EP7_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
}

static void UsbSetAltInterface1(void)
{
	/* EP2 ==> Int IN endpoint, address 1 */
    USBD_CONFIG_EP(EP2, USBD_CFG_EPMODE_IN | 1);
    /* Buffer offset for EP2 */
    USBD_SET_EP_BUF_ADDR(EP2, EP2_BUF_BASE);
	USBD_SET_PAYLOAD_LEN(EP2, 0);

    /* EP3 ==> Int IN endpoint, address 2 */
    USBD_CONFIG_EP(EP3, USBD_CFG_EPMODE_IN | 2);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP3, EP3_BUF_BASE);
	USBD_SET_PAYLOAD_LEN(EP3, 0);

    /* EP4 ==> Int IN endpoint, address 3 */
    USBD_CONFIG_EP(EP4, USBD_CFG_EPMODE_IN | 3);
    /* Buffer offset for EP4 */
    USBD_SET_EP_BUF_ADDR(EP4, EP4_BUF_BASE);
	USBD_SET_PAYLOAD_LEN(EP4, 0);

    /* EP5 ==> Int IN endpoint, address 4 */
    USBD_CONFIG_EP(EP5, USBD_CFG_EPMODE_IN | 4);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP5, EP5_BUF_BASE);
	USBD_SET_PAYLOAD_LEN(EP5, 0);

	/* EP6 ==> Bulk IN endpoint, address 5 */
    USBD_CONFIG_EP(EP6, USBD_CFG_EPMODE_IN | 5);
    /* Buffer offset for EP6 */
    USBD_SET_EP_BUF_ADDR(EP6, EP6_BUF_BASE);

    /* EP7==> Bulk Out endpoint, address 5 */
    USBD_CONFIG_EP(EP7, USBD_CFG_EPMODE_OUT | 5);
    /* Buffer offset for EP7 */
    USBD_SET_EP_BUF_ADDR(EP7, EP7_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
}

static void UsbSetAltInterface(uint32_t u32AltInterface)
{
	if (u32AltInterface <= 1) {
		usb_interface_alt = u32AltInterface;

		if (usb_interface_alt == 0) {
			UsbSetAltInterface0();
		} else if (usb_interface_alt == 1) {
			UsbSetAltInterface1();
		}

		LogDebug("UsbSetAltInterface : %d\n", usb_interface_alt);
	}
}

void UsbTransferInit(void)
{
	USBD_Open(&gsInfo, UsbClassRequest, UsbSetAltInterface);

 	/* Init setup packet buffer */
    /* Buffer for setup packet -> [0 ~ 0x7] */
    USBD->BUFSEG = SETUP_BUF_BASE;

    /*****************************************************/
    /* EP0 ==> control IN endpoint, address 0 */
    USBD_CONFIG_EP(EP0, USBD_CFG_CSTALL | USBD_CFG_EPMODE_IN | 0);
    /* Buffer range for EP0 */
    USBD_SET_EP_BUF_ADDR(EP0, EP0_BUF_BASE);

    /* EP1 ==> control OUT endpoint, address 0 */
    USBD_CONFIG_EP(EP1, USBD_CFG_CSTALL | USBD_CFG_EPMODE_OUT | 0);
    /* Buffer range for EP1 */
    USBD_SET_EP_BUF_ADDR(EP1, EP1_BUF_BASE);

    /*****************************************************/
	UsbSetAltInterface(0);

	NVIC_EnableIRQ(USBD_IRQn);
}

void UsbTransferStart(void)
{
	USBD_Start();
}

void UsbTransferHandleEventInterfaceAlt0(void)
{
	uint32_t u32EpStat;
	uint32_t u32EpStat2;

	static uint32_t u32Ep3StatOld = 0;
	static uint32_t u32Ep5StatOld = 0;
	static uint32_t u32Ep7StatOld = 0;

	uint32_t u32Ep3StatNew;
	uint32_t u32Ep5StatNew;
	uint32_t u32Ep7StatNew;

	u32EpStat = (uint32_t)(USBD->EPSTS);
	u32EpStat2 = (uint32_t)(USBD->EPSTS2);

	u32Ep3StatNew = u32EpStat & USBD_EPSTS_EPSTS3_Msk;
	if (u32Ep3StatNew != u32Ep3StatOld) {
		u32Ep3StatOld = u32Ep3StatNew;
		if ((u32Ep3StatNew == (0x02 << USBD_EPSTS_EPSTS3_Pos))
			|| (u32Ep3StatNew == (0x06 << USBD_EPSTS_EPSTS3_Pos)))
		{
			u32RxSize3 = USBD_GET_PAYLOAD_LEN(EP3);
			USBD_MemCopy(u8RxBuf3, (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP3)), u32RxSize3);
			HmdMagneticHandleCmd((uint8_t *)u8RxBuf3, u32RxSize3, u8TxBuf2, sizeof(u8TxBuf2), &u32TxSize2);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP2)), u8TxBuf2, u32TxSize2);
			USBD_SET_PAYLOAD_LEN(EP2, u32TxSize2);

			u32RxSize3 = 0;
			USBD_SET_PAYLOAD_LEN(EP3, EP3_MAX_PKT_SIZE);
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP3);
		}
	}

	u32Ep5StatNew = u32EpStat & USBD_EPSTS_EPSTS5_Msk;
	if (u32Ep5StatNew != u32Ep5StatOld) {
		u32Ep5StatOld = u32Ep5StatNew;
		if ((u32Ep5StatNew == (0x02 << USBD_EPSTS_EPSTS5_Pos))
			|| (u32Ep5StatNew == (0x06 << USBD_EPSTS_EPSTS5_Pos)))
		{
			u32RxSize5 = USBD_GET_PAYLOAD_LEN(EP5);
			USBD_MemCopy(u8RxBuf5, (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP5)), u32RxSize5);
			HmdGsensorHandleCmd((uint8_t *)u8RxBuf5, u32RxSize5, u8TxBuf4, sizeof(u8TxBuf4), &u32TxSize4);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP4)), u8TxBuf4, u32TxSize4);
			USBD_SET_PAYLOAD_LEN(EP4, u32TxSize4);

			u32RxSize5 = 0;
			USBD_SET_PAYLOAD_LEN(EP5, EP5_MAX_PKT_SIZE);
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP5);
		}
	}

	u32Ep7StatNew = u32EpStat2 & USBD_EPSTS2_EPSTS7_Msk;
	if (u32Ep7StatNew != u32Ep7StatOld) {
		u32Ep7StatOld = u32Ep7StatNew;
		if ((u32Ep7StatNew == (0x02 << USBD_EPSTS2_EPSTS7_Pos))
			|| (u32Ep7StatNew == (0x06 << USBD_EPSTS2_EPSTS7_Pos)))
		{
			u32RxSize7 = USBD_GET_PAYLOAD_LEN(EP7);
			USBD_MemCopy(u8RxBuf7, (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP7)), u32RxSize7);
			HmdGyroHandleCmd((uint8_t *)u8RxBuf7, u32RxSize7, u8TxBuf6, sizeof(u8TxBuf6), &u32TxSize6);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP6)), u8TxBuf6, u32TxSize6);
			USBD_SET_PAYLOAD_LEN(EP6, u32TxSize6);

			u32RxSize7 = 0;
			USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP7);
		}
	}
}

void UsbTransferHandleEventInterfaceAlt1(void)
{
	uint32_t u32EpStat2;

	static uint32_t u32Ep7StatOld = 0;
	uint32_t u32Ep7StatNew;

	u32EpStat2 = (uint32_t)(USBD->EPSTS2);

	u32Ep7StatNew = u32EpStat2 & USBD_EPSTS2_EPSTS7_Msk;
	if (u32Ep7StatNew != u32Ep7StatOld) {
		u32Ep7StatOld = u32Ep7StatNew;
		if ((u32Ep7StatNew == (0x02 << USBD_EPSTS2_EPSTS7_Pos))
			|| (u32Ep7StatNew == (0x06 << USBD_EPSTS2_EPSTS7_Pos)))
		{
			u32RxSize7 = USBD_GET_PAYLOAD_LEN(EP7);
			USBD_MemCopy(u8RxBuf7, (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP7)), u32RxSize7);
			HmdDispRspXfer((uint8_t *)u8RxBuf7, u32RxSize7, u8TxBuf6, sizeof(u8TxBuf6), &u32TxSize6);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP6)), u8TxBuf6, u32TxSize6);
			USBD_SET_PAYLOAD_LEN(EP6, u32TxSize6);

			u32RxSize7 = 0;
			USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP7);
		}
	}
}

void UsbTransferHandleEvent(void)
{
	if (usb_interface_alt == 0) {
		UsbTransferHandleEventInterfaceAlt0();
	} else if (usb_interface_alt == 1) {
		UsbTransferHandleEventInterfaceAlt1();
	}
}

void UsbTransferHandleSensorUpdate(void)
{
	uint32_t u32EpStat;

	uint32_t u32Ep2Stat;
	uint32_t u32Ep3Stat;
	uint32_t u32Ep4Stat;
	uint32_t u32Ep5Stat;

	u32EpStat = (uint32_t)(USBD->EPSTS);

	u32Ep2Stat = u32EpStat & USBD_EPSTS_EPSTS2_Msk;
	u32Ep3Stat = u32EpStat & USBD_EPSTS_EPSTS3_Msk;
	u32Ep4Stat = u32EpStat & USBD_EPSTS_EPSTS4_Msk;
	u32Ep5Stat = u32EpStat & USBD_EPSTS_EPSTS5_Msk;

	if (usb_interface_alt == 1) {
		if (u32Ep2Stat == (0x00 << USBD_EPSTS_EPSTS2_Pos)) {
			HmdGsensorRspPoll((uint8_t *)u8TxBuf2,  &u32TxSize2);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP2)), u8TxBuf2, u32TxSize2);
			USBD_SET_PAYLOAD_LEN(EP2, u32TxSize2);
		}

		if (u32Ep3Stat == (0x00 << USBD_EPSTS_EPSTS3_Pos)) {
			HmdGyroRspPoll((uint8_t *)u8TxBuf3,  &u32TxSize3);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP3)), u8TxBuf3, u32TxSize3);
			USBD_SET_PAYLOAD_LEN(EP3, u32TxSize3);
		}

		if (u32Ep4Stat == (0x00 << USBD_EPSTS_EPSTS4_Pos)) {
			HmdMagneticRspPoll((uint8_t *)u8TxBuf4,  &u32TxSize4);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP4)), u8TxBuf4, u32TxSize4);
			USBD_SET_PAYLOAD_LEN(EP4, u32TxSize4);
		}

		if (u32Ep5Stat == (0x00 << USBD_EPSTS_EPSTS5_Pos)) {
			HmdEventRspPoll((uint8_t *)u8TxBuf5,  &u32TxSize5);
			USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP5)), u8TxBuf5, u32TxSize5);
			USBD_SET_PAYLOAD_LEN(EP5, u32TxSize5);
		}
	}
}
